/*
 *  Simbad - Robot Simulator
 *  Copyright (C) 2004 Louis Hugues
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 -----------------------------------------------------------------------------
 * $Author: sioulseuguh $ 
 * $Date: 2005/03/17 17:49:37 $
 * $Revision: 1.13 $
 * $Source: /cvsroot/simbad/src/simbad/sim/RangeSensorBelt.java,v $
 * 21/02/2005 measurrment init value is Double.POSITIVE_INFINITY.
 * 
 * History:
 * BUGFIX Louis 01/10/2006  correct getLeftQuadrantMeasurement.
 * BUGFIX Louis 30/12/2006  now range sensor belt returns infinity or MaxRange(default) if no obstacle. 
 */
package simbad.sim;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Font;
import java.awt.Graphics;
import java.text.DecimalFormat;

import javax.media.j3d.Appearance;
import javax.media.j3d.ColoringAttributes;
import javax.media.j3d.GeometryArray;
import javax.media.j3d.LineArray;
import javax.media.j3d.Material;
import javax.media.j3d.Node;
import javax.media.j3d.PickCylinderRay;
import javax.media.j3d.PickSegment;
import javax.media.j3d.SceneGraphPath;
import javax.media.j3d.Shape3D;
import javax.media.j3d.Transform3D;
import javax.swing.JPanel;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

/**
 * This class models a circular belt of range sensors : Sonar , Bumpers (future :
 * Laser, Ir). Sensors are arranged circularly around the robot. User can access
 * to the measurement and hits state of each sensor individualy or obtain an
 * average measurement in a quadrant. <br>
 * <br>
 * Note that the sensors are not affected by noise. however you can easily add
 * some gaussian noise with the java.util.Random class. <br>
 * <code> Random generator = new Random(seed); </code> <br>
 * and<br>
 * <code> value = sonars.getMeasurement(0)+generator.nextGaussian()*stddev;</code>
 * <br>
 * <br>
 * <br>
 * Implementation notes : We use java 3D picking feature to emulate sensing. A
 * PickCylinderRay is used on each update to test whether there is an potential
 * obstacle. Each ray is then checked with a PickSegement.
 */
public class RangeSensorBelt extends PickSensor {

	private int type;
	private float maxRange;
	private float radius;
	private int nbSensors;
	/** for storing results */
	private double measurements[];
	private boolean hits[];
	private boolean oneHasHit;
	/** angular position (deduced from positions infos) */
	private double angles[];
	/** * position of each sensor relative to center */
	private Vector3d positions[];
	/** direction vector of each sensor - relative to sensor position. */
	private Vector3d directions[];

	private Transform3D t3d;
	// private Transform3D t3d_2;
	private Point3d start;
	private Point3d end;
	private Color3f color;
	/** for picking */
	private PickCylinderRay pickCylinder;
	private PickSegment pickSegment;
	private Point3d cylinderStart;
	private float cylinderRadius;
	private Vector3d cylinderDirection;

	private int flags;
	// constants
	public final static int TYPE_SONAR = 0;
	public final static int TYPE_IR = 1;
	public final static int TYPE_LASER = 2;
	public final static int TYPE_BUMPER = 3;

	// FLAGS
	public final static int FLAG_SHOW_FULL_SENSOR_RAY = 0X01;
	public final static int FLAG_RETURN_INFINITY = 0X02;

	/**
	 * Constructs a RangeSensorBelt. The sensor type can be either
	 * TYPE_BUMPER,TYPE_SONAR,TYPE_IR or TYPE_LASER. Ranges are measured from
	 * the belt perimeter (not from the belt center).
	 * 
	 * @param radius -
	 *            the radius of the belt.
	 * @param minRange -
	 *            the minimal range of each sensor ray. Not used for
	 *            TYPE_BUMPER.
	 * @param maxRange -
	 *            the maximal range of each sensor ray. Not used for
	 *            TYPE_BUMPER.
	 * @param nbsensors -
	 *            the number of sensors in the belt (typically 4,6,12,24 or 36).
	 * @param type -
	 *            to specify the sensor behavior
	 * @param flags
	 *            FLAG_RETURN_INFINITY Set this flag if you want sensor to
	 *            return Infinity if no obstacles. (default return maxRange)
	 */
	public RangeSensorBelt(float radius, float minRange, float maxRange, int nbsensors, int type, int flags) {
		// compute angles ,positions , directions
		positions = new Vector3d[nbsensors];
		directions = new Vector3d[nbsensors];
		Vector3d frontPos = new Vector3d(radius, 0, 0);
		Vector3d frontDir = new Vector3d(maxRange, 0, 0);
		angles = new double[nbsensors];
		Transform3D transform = new Transform3D();
		for (int i = 0; i < nbsensors; i++) {
			angles[i] = i * 2 * Math.PI / nbsensors;
			transform.setIdentity();
			transform.rotY(angles[i]);
			Vector3d pos = new Vector3d(frontPos);
			transform.transform(pos);
			positions[i] = pos;
			Vector3d dir = new Vector3d(frontDir);
			transform.transform(dir);
			directions[i] = dir;
		}
		initialize(radius, maxRange, nbsensors, type, flags);
	}

	/**
	 * Constructs a RangeSensorBelt. The sensor type can be either
	 * TYPE_BUMPER,TYPE_SONAR,TYPE_IR or TYPE_LASER.
	 * 
	 * @param positions :
	 *            the position of each sensor relative to belt center.
	 * @param directions :
	 *            the sensing ray direction of each sensor relative to sensor
	 *            positions. the magnitude of the vector corresponds to the max
	 *            range.
	 * @param flags
	 *            FLAG_RETURN_INFINITY Set this flag if you want sensor to
	 *            return Infinity if no obstacles. (default return maxRange)
	 */
	public RangeSensorBelt(Vector3d[] positions, Vector3d[] directions, int type, int flags) {

		int nbsensors = positions.length;

		// compute angles
		float radius = Float.MIN_VALUE;
		float maxRange = Float.MIN_VALUE;
		Vector3d frontVector = new Vector3d(1, 0, 0);
		angles = new double[nbsensors];
		for (int i = 0; i < nbsensors; i++) {
			Vector3d v = positions[i];
			angles[i] = v.angle(frontVector);
			double norm = v.length();
			// find the max radius
			if (norm > radius)
				radius = (float) norm;
			double range = directions[i].length();
			if (range > maxRange)
				maxRange = (float) range;

		}
		this.directions = directions;
		this.positions = positions;

		initialize(radius, maxRange, nbsensors, type, flags);

	}

	private void initialize(float radius, float maxRange, int nbsensors, int type, int flags) {
		this.flags = flags;
		this.nbSensors = nbsensors;
		this.maxRange = maxRange;
		this.radius = radius;
		this.type = type;
		// reserve to avoid gc.
		t3d = new Transform3D();
		// t3d_2 = new Transform3D();
		pickSegment = new PickSegment();
		pickCylinder = new PickCylinderRay();
		cylinderDirection = new Vector3d(0.0, 0.5, 0.0);
		cylinderRadius = maxRange + radius;
		cylinderStart = new Point3d(0f, 0f, 0.f);
		start = new Point3d();
		end = new Point3d();

		if (type == TYPE_BUMPER)
			color = new Color3f(1.0f, 0f, 0f);
		else
			color = new Color3f(1.0f, 0.5f, 0.25f);

		measurements = new double[nbsensors];
		hits = new boolean[nbsensors];
		for (int i = 0; i < nbsensors; i++) {
			// BUGFIX Louis 30-dec-2006
			if ((flags & FLAG_RETURN_INFINITY) != 0)
				measurements[i] = Double.POSITIVE_INFINITY;
			else
				measurements[i] = maxRange;

			hits[i] = false;
		}
		oneHasHit = false;
		create3D();

	}

	private void create3D() {
		super.create3D(true);
		// construct sensor body - a line for each individual sensor ray.
		Point3d[] coords = new Point3d[nbSensors * 2];
		for (int i = 0; i < nbSensors; i++) {
			Point3d start = new Point3d(positions[i]);
			coords[i * 2] = start;
			Point3d end = new Point3d(start);
			Point3d direction = new Point3d(directions[i]);
			if (((flags & FLAG_SHOW_FULL_SENSOR_RAY) == 0) && (type != TYPE_BUMPER))
				direction.scale(0.05f); // just
			// a
			// small
			// ray
			end.add(direction);
			coords[i * 2 + 1] = end;

		}
		LineArray line = new LineArray(coords.length, GeometryArray.COORDINATES);
		line.setCoordinates(0, coords);

		Appearance appear = new Appearance();
		Material material = new Material();

		ColoringAttributes ca = new ColoringAttributes();
		ca.setColor(color);

		appear.setColoringAttributes(ca);
		appear.setMaterial(material);
		Shape3D shape = new Shape3D(line, appear);
		shape.setCollidable(false);
		shape.setPickable(false);
		addChild(shape);

	}

	@Override
	protected void update() {
		oneHasHit = false;
		for (int s = 0; s < nbSensors; s++) {
			hits[s] = false;
			// BUGFIX Louis 30-dec-2006
			if ((flags & FLAG_RETURN_INFINITY) != 0)
				measurements[s] = Double.POSITIVE_INFINITY;
			else
				measurements[s] = maxRange;

		}
		// update the pickShape with current position
		// TODO factor this getLocalToVWorld
		group.getLocalToVworld(t3d);
		cylinderStart.set(0.0f, 0.0f, 0.f);
		cylinderDirection.set(0.0, 0.5, 0.0);

		// set a pickCylinder around the belt
		t3d.transform(cylinderStart);
		t3d.transform(cylinderDirection);
		pickCylinder.set(cylinderStart, cylinderDirection, cylinderRadius);

		// pick possibly intersecting shapes
		// rem: pickAllSorted costs too much
		SceneGraphPath[] picked = pickableSceneBranch.pickAll(pickCylinder);
		boolean intersect = false;
		double minDist;
		double[] dist = new double[1];
		if (picked != null) {

			// now check each sensor ray
			for (int s = 0; s < nbSensors; s++) {
				start.set(positions[s]);
				end.set(start);
				end.add(directions[s]);

				t3d.transform(start);
				t3d.transform(end);

				pickSegment.set(start, end);
				// find the nearest
				minDist = Double.MAX_VALUE;

				intersect = false;
				// Pick again but on the segment

				picked = pickableSceneBranch.pickAll(pickSegment);
				if (picked != null) {
					// for all picked objects
					for (int i = 0; i < picked.length; i++) {
						Node obj = picked[i].getObject();
						if (obj instanceof Shape3D) {
							if (((Shape3D) obj).intersect(picked[i], pickSegment, dist)) {
								if (dist[0] < minDist) {
									minDist = dist[0];
									intersect = true;
								}
							}
						}
					}
				}
				hits[s] = intersect;
				oneHasHit |= intersect;
				if (intersect) {

					measurements[s] = minDist;
					// System.out.println ("Sensor "+s+"="+(minDist));
				}
			}
		}

	}

	/**
	 * Returns the last measure collected for the individual sensor. Measurement
	 * is made from the circle perimeter.
	 * 
	 * @param sensorNum
	 *            num of the sensor.
	 * @return the range measurment.
	 */
	public double getMeasurement(int sensorNum) {
		return measurements[sensorNum];
	}

	/**
	 * Returns the averaged measure of the sensors situated in the front
	 * quadrant: [-PI/4,PI/4].
	 * 
	 * @return the averaged measurment.
	 */
	public double getFrontQuadrantMeasurement() {
		return (getQuadrantMeasurement(0, Math.PI / 4) + getQuadrantMeasurement(7 * Math.PI / 4, Math.PI * 2)) / 2.0;
	}

	/**
	 * Returns the averaged measure of the sensors situated in the front left
	 * quadrant: [0,PI/4].
	 * 
	 * @return the averaged measurment.
	 */
	public double getFrontLeftQuadrantMeasurement() {
		return getQuadrantMeasurement(0, Math.PI / 2);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the front right
	 * quadrant: [3PI/2,2*PI].
	 * 
	 * @return the averaged measurment.
	 */
	public double getFrontRightQuadrantMeasurement() {
		return getQuadrantMeasurement(3 * Math.PI / 2.0, 2 * Math.PI);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the left
	 * quadrant: [PI/4,PI*3/4].
	 * 
	 * @return the averaged measurment.
	 */
	public double getLeftQuadrantMeasurement() {
		return getQuadrantMeasurement(Math.PI / 4, 3 * Math.PI / 4);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the bacck left
	 * quadrant: [PI/2,PI].
	 * 
	 * @return the averaged measurment.
	 */
	public double getBackLeftQuadrantMeasurement() {
		return getQuadrantMeasurement(Math.PI / 2, Math.PI);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the back
	 * quadrant: [3PI/4,4*PI/4].
	 * 
	 * @return the averaged measurment.
	 */
	public double getBackQuadrantMeasurement() {
		return getQuadrantMeasurement(3 * Math.PI / 4, 5 * Math.PI / 4);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the back right
	 * quadrant: [PI,3*PI/2].
	 * 
	 * @return the averaged measurment.
	 */
	public double getBackRightQuadrantMeasurement() {
		return getQuadrantMeasurement(Math.PI, 3 * Math.PI / 2);
	}

	/**
	 * Returns the averaged measure of the sensors situated in the right
	 * quadrant: [5*PI/4,7*PI/4].
	 * 
	 * @return the averaged measurment.
	 */
	public double getRightQuadrantMeasurement() {
		return getQuadrantMeasurement(5 * Math.PI / 4, 7 * Math.PI / 4);
	}

	/**
	 * Returns the averaged measure of the sensors situated in quadrant
	 * [minAngle,maxAngle].
	 * 
	 * @param minAngle
	 *            in radians the right limit of the quadrant.
	 * @param maxAngle
	 *            in radians the left limit of the quadrant.
	 * @return the averaged measurment.
	 */
	public double getQuadrantMeasurement(double minAngle, double maxAngle) {
		double sum = 0.0, n = 0;
		for (int i = 0; i < nbSensors; i++) {
			if ((angles[i] >= minAngle) && (angles[i] <= maxAngle)) {
				if (hits[i]) {
					n += 1.0;
					sum += measurements[i];
				}
			}
		}
		if (n == 0) {
			// BUGFIX Louis 30-dec-2006
			if ((flags & FLAG_RETURN_INFINITY) != 0)
				return Double.POSITIVE_INFINITY;
			else
				return maxRange;
		} else
			return sum / n;
	}

	/**
	 * Returns number of sensor hits in the front quadrant: [-PI/4,PI/4].
	 * 
	 * @return the number of hits.
	 */
	public int getFrontQuadrantHits() {
		return (getQuadrantHits(0, Math.PI / 4) + getQuadrantHits(7 * Math.PI / 4, Math.PI * 2));
	}

	/**
	 * Returns number of sensor hits in the front left quadrant: [0,PI/4].
	 * 
	 * @return the number of hits.
	 */
	public int getFrontLeftQuadrantHits() {
		return getQuadrantHits(0, Math.PI / 2);
	}

	/**
	 * Returns number of sensor hits in the front right quadrant: [3PI/2,2*PI].
	 * 
	 * @return the number of hits.
	 */
	public int getFrontRightQuadrantHits() {
		return getQuadrantHits(3 * Math.PI / 2.0, 2 * Math.PI);
	}

	/**
	 * Returns number of sensor hits in the left quadrant: [PI/4,PI*3/4].
	 * 
	 * @return the number of hits.
	 */
	public int getLeftQuadrantHits() {
		return getQuadrantHits(Math.PI / 4, 3 * Math.PI / 4);
	}

	/**
	 * Returns number of sensor hits in the back left quadrant: [PI/2,PI].
	 * 
	 * @return the number of hits.
	 */
	public int getBackLeftQuadrantHits() {
		return getQuadrantHits(Math.PI / 2, Math.PI);
	}

	/**
	 * Returns number of sensor hits in the back quadrant: [3PI/4,5PI/4].
	 * 
	 * @return the number of hits.
	 */
	public int getBackQuadrantHits() {
		return getQuadrantHits(3 * Math.PI / 4, 5 * Math.PI / 4);
	}

	/**
	 * Returns number of sensor hits in the back right quadrant: [PI,3PI/2].
	 * 
	 * @return the number of hits.
	 */
	public int getBackRightQuadrantHits() {
		return getQuadrantHits(Math.PI, 3 * Math.PI / 2);
	}

	/**
	 * Returns number of sensor hits in the right quadrant: [5PI/4,7PI/4].
	 * 
	 * @return the number of hits.
	 */
	public int getRightQuadrantHits() {
		return getQuadrantHits(5 * Math.PI / 4, 7 * Math.PI / 4);
	}

	/**
	 * Returns number of hits in quadrant [minAngle,maxAngle].
	 * 
	 * @param minAngle
	 *            in radians the right limit of the quadrant.
	 * @param maxAngle
	 *            in radians the left limit of the quadrant.
	 * @return the number of hits.
	 */
	public int getQuadrantHits(double minAngle, double maxAngle) {
		int n = 0;
		for (int i = 0; i < nbSensors; i++) {
			if ((angles[i] >= minAngle) && (angles[i] <= maxAngle)) {
				if (hits[i])
					n++;
			}
		}
		return n;
	}

	/**
	 * Returns the hit state of the sensor.
	 * 
	 * @param sensorNum
	 *            num of the sensor.
	 * @return true if the sensor ray has hit an obstacle
	 */
	public boolean hasHit(int sensorNum) {
		return hits[sensorNum];
	}

	/**
	 * Returns true if one of the sensors has hit.
	 * 
	 * @return true if one ray has hit an obstacle
	 */
	public boolean oneHasHit() {
		return oneHasHit;
	}

	/**
	 * Return the number of individual sensor in the belt.
	 * 
	 * @return the number of sensors.
	 */
	public int getNumSensors() {
		return nbSensors;
	}

	/**
	 * Returns the angle of this sensor.
	 * 
	 * @param sensorNum -
	 *            num of the sensor.
	 * @return the angle in radians.
	 */
	public double getSensorAngle(int sensorNum) {
		return angles[sensorNum];
	}

	/**
	 * Returns the maximum sensing range in meters.
	 * 
	 * @return the maximum range.
	 */
	public float getMaxRange() {
		return maxRange;
	}

	@Override
	public JPanel createInspectorPanel() {
		return new RangeSensorBeltJPanel();
	}

	/**
	 * Return sensor flags.
	 */
	public int getFlags() {
		return flags;
	}

	/**
	 * A JPanel Inner class for displaying the sensor belt rays in 2d.
	 */
	private class RangeSensorBeltJPanel extends JPanel {

		private static final long serialVersionUID = 1L;
		Font font;
		int lineSize = 8;
		DecimalFormat format;
		final static int IMAGE_SIZEX = 200;
		final static int IMAGE_SIZEY = 100;

		public RangeSensorBeltJPanel() {
			Dimension d = new Dimension(IMAGE_SIZEX, IMAGE_SIZEY);
			setPreferredSize(d);
			setMinimumSize(d);
			font = new Font("Arial", Font.PLAIN, lineSize - 1);
			// display format for numbers
			format = new DecimalFormat();
			format.setMaximumFractionDigits(3);
			format.setMinimumFractionDigits(2);
			format.setPositivePrefix("");
			format.setMinimumIntegerDigits(1);
		}

		/** Caution not synchronised */

		@Override
		protected void paintComponent(Graphics g) {
			super.paintComponent(g);
			g.setFont(font);
			// Color c;
			g.setColor(Color.LIGHT_GRAY);
			g.fillRect(0, 0, IMAGE_SIZEX, IMAGE_SIZEY);
			g.setColor(Color.GRAY);
			int x = IMAGE_SIZEX / 2;
			int y = 0;
			for (int i = 0; i < nbSensors; i++) {
				y += lineSize;
				if (y > IMAGE_SIZEY) {
					y = lineSize;
					x += 50;
				}
				if (type == TYPE_BUMPER)
					g.drawString("[" + i + "] " + hits[i], x, y);
				else
					g.drawString("[" + i + "] " + format.format(measurements[i]), x, y);
			}
			int cx, cy;
			cx = cy = IMAGE_SIZEX / 4;
			// display factor
			float f = cx / (maxRange + radius);
			int r = (int) (radius * f);
			g.setColor(Color.BLUE);
			g.drawOval(cx - r, cy - r, 2 * r, 2 * r);
			g.drawLine(cx, cy, cx + r, cy);
			// draw each ray
			for (int i = 0; i < nbSensors; i++) {

				int x1 = (int) (positions[i].x * f);
				int y1 = (int) (positions[i].z * f);
				if (type == TYPE_BUMPER) {
					g.setColor(Color.BLUE);
					g.drawRect(cx + x1 - 1, cy + y1 - 1, 2, 2);
				}
				g.setColor(Color.RED);
				if (hits[i]) {
					double ratio = measurements[i] / directions[i].length();

					int x2 = x1 + (int) (directions[i].x * ratio * f);
					int y2 = y1 + (int) (directions[i].z * ratio * f);
					if (type == TYPE_BUMPER) {
						g.fillOval(cx + x1 - 1, cy + y1 - 1, 3, 3);
					} else
						g.drawLine(cx + x1, cy + y1, cx + x2, cy + y2);
				}
			}
		}
	}
}
